#include <custom_msg/CMD.h>
#include <ros/ros.h>
#include <std_msgs/Int8.h>

#include <cmath>
#include <cstdlib>  //string转化为double
#include <iomanip>  //保留有效小数
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#include "commons/rabbit_consts.h"

using namespace std;

int main(int argc, char** argv) {
  ros::init(argc, argv, "parse_cmd_for_flashros");
  ros::NodeHandle nh;
  ros::Publisher rgb_ir_publisher =
      nh.advertise<std_msgs::Int8>("/flashros/cmd/channel", 1);
  ros::Publisher rgb_focus_publisher =
      nh.advertise<std_msgs::Int8>("/flashros/cmd/focus/rgb", 1);
  ros::Publisher ir_focus_publisher =
      nh.advertise<std_msgs::Int8>("/flashros/cmd/focus/ir", 1);

  auto chatterCallback = [&](const custom_msg::CMDConstPtr& msg) {
    int cmd_type = msg->cmd_type;
    int val_type = msg->val_type;
    ROS_WARN_STREAM("Accept cmd " << cmd_type << " " << val_type);
    std_msgs::Int8 parsed_msg;
    if (cmd_type == MSG_CMD_TYPE_SUMBOY) {
      switch (val_type) {
        case MSG_CMD_TYPE_SUMBOY_HW: {
          parsed_msg.data = 1;
          rgb_ir_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB: {
          parsed_msg.data = 0;
          rgb_ir_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS1: {
          parsed_msg.data = 0;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS2: {
          parsed_msg.data = 1;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS3: {
          parsed_msg.data = 2;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS4: {
          parsed_msg.data = 3;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS5: {
          parsed_msg.data = 4;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS6: {
          parsed_msg.data = 5;
          ir_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1: {
          parsed_msg.data = 0;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2: {
          parsed_msg.data = 1;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3: {
          parsed_msg.data = 2;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4: {
          parsed_msg.data = 3;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5: {
          parsed_msg.data = 4;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6: {
          parsed_msg.data = 5;
          rgb_focus_publisher.publish(parsed_msg);
          break;
        }
      }
    }
  };

  auto sub = nh.subscribe<custom_msg::CMD>(TOPIC_CMD, 1, chatterCallback);
  ros::spin();
}
